/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
#ifndef MCL_INTERFACE_H_
#define MCL_INTERFACE_H_

#include <time.h>
#include <stdio.h>
#include "linelib.h"
#include "mcl_defines.h"
#include "particleMath.h"
#include "mapHandling.h"

//#define MCL_DEBUG
#define NUMBER_OF_PARTICLES 500

typedef struct __TMCLData{
	TLineVector map;				// Apriori Map
	bool isMapLoaded;               // Tells if the map is loaded
	TPoseParticleDistribution PPD;	// Particle distribution
	float Q[3];						// Uncertainty
	bool isInitialized;				// tells if particle filter is initialized
	float v[3];						// Motion between last position and current position (in absolute coordinates!)
	float *scan;					// Current measurement (x,y) vector
	float Qini[3];					// Uncertainty of initial position
	int numOfSIRupdates;			// How many updates without SIR
	TPose meanPos;					// Average of pose distribution	
	TPose varPos;					// Variance of the pose distribution
	int timesBadLik;                // The number of times the likelyhood has been bad
	TPose odoOld;                   // The odometry value stored after estimation  
}TMCLData;


/**
* A help function to tell if it is ok to use the MCL
*/
bool mcl_isOk(TMCLData &MCL);

/**
* Does the initialization of the variables... MUST be called in the beginning else segfault ;)
*/
void initMCL(TMCLData &MCL);

/**
* Loads the map for the MCL from the file 
*/ 
void mcl_loadMapFromFile(TMCLData &MCL, const char *filename);

/**
* Loads the map from server command 
*/
void mcl_loadMapFromServer(TMCLData &MCL, char *buf);


/**
* Initialises the particle distribution to some given initial pose
*/
void mcl_initializeDistribution(TMCLData &MCL, TPose initialPose, int numberOfParticles);


void scan2MCLscan(float *r, float *mclR);
/**
* The MCL Calculation
* Inputs are current scan, The pose where the scan has been taken and the MCL datastructure
* @param *scan      The range scan
* @param current    Pose where the scan has been taken according to the odometry
* @param &MCL       Reference to the MCL data structure
*/
void calculateMCL(float *scan, TPose current, TMCLData &MCL);

void freeMCL(TMCLData &MCL);

#endif
